
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <signal.h>
#include "serial.h"
#include "timer.h"

#define BUFFER_SIZE 8
#define RANGING_TIMES 10

void initialize(int baud_rate,int send_rate);
void  handle1();
void  handle2();
void  handle3();
void   signal_handler_IO   (void);

int fd;
fd_set rd;
volatile int packet_send,status,is_write,is_timeout,wait_flag;
long send_interval;
unsigned char buffer_s[BUFFER_SIZE];
unsigned char buffer_r[BUFFER_SIZE];

int main(int argc,char **argv)
{
    int nwrite;
	unsigned int baud_rate=atoi(argv[1]);
	unsigned int send_rate=atoi(argv[2]);
    
    initialize(baud_rate,send_rate);


	while(1);
    close(fd);
    return 1;
}

void initialize(int baud_rate,int send_rate)
{
	int ret;	  

  	if((fd = open_serial(fd,1)) < 0)
    {
        perror("open_serial error");
        exit(1);
    }

    if((ret=set_option(fd,baud_rate,8,'N',1)) < 0)
    {
        perror("set_option error");
        exit(1);
    }

	struct   sigaction   saio; 
	saio.sa_handler=signal_handler_IO;                
	sigemptyset(&saio.sa_mask);     
	saio.sa_flags=0;                
	saio.sa_restorer=NULL;             
	sigaction(SIGIO,&saio,NULL);          
	fcntl(fd,F_SETOWN,getpid());             
	fcntl(fd,F_SETFL,FASYNC);
	struct timespec s,r,d;
	/*
	struct sigevent evp1,evp2;
	struct itimerspec ts1,ts2;
	timer_t timer1,timer2;
	
	evp1.sigev_value.sival_ptr = &timer1;
 	evp1.sigev_notify = SIGEV_SIGNAL;
	evp1.sigev_signo = SIGUSR1;
	signal(SIGUSR1, handle3);
 	
	ts1.it_interval.tv_sec = 0;
	ts1.it_interval.tv_nsec =500000*1000;
	ts1.it_value.tv_sec = 0;
	ts1.it_value.tv_nsec =500000*1000;

	if(ret= timer_create(CLOCK_REALTIME, &evp1, &timer1))
	{
		 perror("timer1 create error");
		 exit(1);
	}
	if(ret=timer_settime(timer1, 0, &ts1, NULL))
	{
		perror("timer1 settime error");
		exit(1);
	}

	struct timespec s,r,d;	
 	int nread,times,t;
	double delay_average=0.0;
	

	
	buffer_s[0]=RANGING_REQ;
		
	t=0;
	for(times=0;times<RANGING_TIMES;times++)
	{	
		is_timeout=0;
		status=0;
		is_write=1;
		ts1.it_interval.tv_sec = 0;
		ts1.it_interval.tv_nsec =500000*1000;
		ts1.it_value.tv_sec = 0;
		ts1.it_value.tv_nsec =500000*1000;
    		write(fd,buffer_s,1);
		if(ret=timer_settime(timer1, 0, &ts1, NULL))
		{
			perror("timer1 settime error");
			exit(1);
		}
		clock_gettime(CLOCK_REALTIME, &s);
		//printf("send %x@:%dsec,%.1fuesc\n",buffer_s[0],s.tv_sec,s.tv_nsec/1000.0);
		while(!is_timeout){
			//is_timeout=0;
			read(fd,buffer_r,1);
			clock_gettime(CLOCK_REALTIME, &r);
			if(1==status){
	 				status=0;
					if(RANGING_RESP==buffer_r[0])
					{
						buffer_r[0]=0;                          
						d=delta_time(s,r);
						//printf("recv@:%dsec,%.1fuesc\n",r.tv_sec,r.tv_nsec/1000.0);
						printf("delay:%.1fuesc\n",d.tv_nsec/2000.0);     
						delay_average+=d.tv_nsec/2000.0;
						t++;
					}  
					break;              
    		}
		//printf("Ranging timeout %d\n",times);                      
  		}
		//sleep(1);
	}
		if(t)		
		delay_average/=t;
		printf("average delay:%.1f\n",delay_average);   
	*/
	// starting TDMA frame
	buffer_s[0]=TDMA_REQ;
	
	char recv_buff[1024];
	//char data_buff[1024];
	//int Len=53;
	int chk_buf,received_data;
	while(1){
		//is_timeout=0;
		//status=0;
		//is_write=1;
		//tcflush(fd,TCIOFLUSH);
		received_data=0;
		/*
		ts1.it_interval.tv_sec = 0;
		ts1.it_interval.tv_nsec =500000*1000;
		ts1.it_value.tv_sec = 0;
		ts1.it_value.tv_nsec =500000*1000;
		
		if(ret=timer_settime(timer1, 0, &ts1, NULL))
		{
			perror("timer1 settime error");
			exit(1);
		}
		*/
		tcflush(fd,TCIOFLUSH);
    	write(fd,buffer_s,1);
    	clock_gettime(CLOCK_REALTIME, &s);
    	printf("request for data\n");
    	if(wait_flag==0)
    	{
    	wait_flag=1; 
    	int readnum;
    	int flag_test = 0;
		while(1)
		{
		readnum=read(fd,recv_buff,1);
		//printf("%x\n",recv_buff[0]);
		if (readnum<0)
		{
		printf("read error\n");
		//break;
		}
			if(flag_test < 3){
				printf("%x\n",recv_buff[0]);
				flag_test ++;
				//getch();
			}
		
		if(recv_buff[0]==TDMA_RESP)
		{
		printf("received frame head\n");
		
		break;
		}
		
		}
		
		// read data from buffer
		int total_read_bytes=0;
		readnum=treadn(fd, recv_buff, 3, 1000*1000);
		total_read_bytes=(int)(recv_buff[0]-'0')*100+(int)(recv_buff[1]-'0')*10+(int)(recv_buff[2]-'0');
		
		printf("packet length:%d\n",total_read_bytes);
		
		int treadnum;
		treadnum=treadn(fd, recv_buff, total_read_bytes+1, 1000*1000);
		
		printf("read treadnum:%d data from buff\n",treadnum);
		
		if (recv_buff[treadnum-1]!=0x00)
		printf("frame error\n");
		clock_gettime(CLOCK_REALTIME, &r);
		
		int actual_read_bytes=0;
		actual_read_bytes=string_len(recv_buff);

		printf("actually received bytes:%d\n",actual_read_bytes);
		
		printf("%s\n",recv_buff);
		d=delta_time(s,r);
		
		printf("delay:%.1fuesc\n",d.tv_nsec/1000.0);
		
		//received all data
		//if(TDMA_RESP==recv_buff[0])
		//{
			//buffer_r[0]=0;
			/*
			for(chk_buf=3;chk_buf<total_read_bytes;chk_buf++)
			{
				if(recv_buff[chk_buf]==0x30)
					received_data++;
				else
					printf("received data error\n");
			}
			*/
			//printf("sucessfully received bytes:%d\n",received_data);
			//if (total_read_bytes==received_data+3)
				
			//else
			//printf("received data number error\n");
			
			//printf("recv@:%dsec,%.1fuesc\n",r.tv_sec,r.tv_nsec/1000.0);
			     
			
		//}
		//else
		//printf("packet error\n"); 
		//memset(recv_buff,0,52);                     
  		}
  		
  		//sleep(5); 
	}	
	/*
	signal(SIGUSR1, handle1);

 	evp2.sigev_value.sival_ptr = &timer2;
 	evp2.sigev_notify = SIGEV_SIGNAL;
	evp2.sigev_signo = SIGUSR2;
	signal(SIGUSR2, handle2);

	ts1.it_interval.tv_sec = 0;
	ts1.it_interval.tv_nsec =(10000000L)/(send_rate/100);
	ts1.it_value.tv_sec = 0;
	ts1.it_value.tv_nsec =(10000000L)/(send_rate/100);

	ts2.it_interval.tv_sec = 5;
	ts2.it_interval.tv_nsec =0;
	ts2.it_value.tv_sec = 5;
	ts2.it_value.tv_nsec =0;

	/*
	if(ret= timer_create(CLOCK_REALTIME, &evp1, &timer1))
	{
		 perror("timer1 create error");
		 exit(1);
	}*/
	/*
	if(ret= timer_create(CLOCK_REALTIME, &evp2, &timer2))
	{
		 perror("timer2 create error");
		 exit(1);
	}		

	if(ret=timer_settime(timer1, 0, &ts1, NULL))
	{
		perror("timer1 settime error");
		exit(1);
	}
	if(ret=timer_settime(timer2, 0, &ts2, NULL))
	{
		perror("timer2 settime error");
		exit(1);
	}
	*/
	return ;
}

ssize_t tread(int fd, void *buf, size_t nbytes, unsigned int timout)
{
int nfds;
fd_set readfds;
struct timeval tv;
tv.tv_sec = 0;
tv.tv_usec = timout;
FD_ZERO(&readfds);
FD_SET(fd, &readfds);
nfds = select(fd+1, &readfds, NULL, NULL, &tv);
if (nfds <= 0) {
if (nfds == 0)
errno = ETIME;
return(-1);
}
return(read(fd, buf, nbytes));
}

ssize_t treadn(int fd, void *buf, size_t nbytes, unsigned int timout)
{
size_t nleft;
ssize_t nread;
nleft = nbytes;
while (nleft > 0) {
if ((nread = tread(fd, buf, nleft, timout)) < 0) {
if (nleft == nbytes)
return(-1); /* error, return -1 */
else
break; /* error, return amount read so far */
} else if (nread == 0) {
break; /* EOF */
}
nleft -= nread;
buf += nread;
}
return(nbytes - nleft); /* return >= 0 */
}

int string_len(char *p)
{
  int k=0;
  while(*p++ != 0x00 )
  	k++;
  return k;
}

void  ranging(void)
{

}



void  handle1()
{
	static unsigned char count=0;	
	//buffer_s[0]=DATA;
    	//write(fd,buffer_s,1);
	packet_send++;
	//printf("[send]%d:%d\n",packet_send,buffer[0]);
	count++;
}

void  handle2()
{
	printf("packet_send:%d\n",packet_send);
	packet_send=0;
}

void  handle3()
{
	is_timeout=1;
}

void   signal_handler_IO(){
	//printf("received SIGIO signal.\n");
	wait_flag = 0;
	if(!is_write){
		status=1;
	}  
	else
		is_write=0;	 
} 
